#include "pfsm_free.h"
#include "axisgroup_robot.h"
#include "axis/axis.h"
#include "safety/io/safeio.h"
#include "zuc/nml_intf/zucpos2.h"
#include "kine/kine.h"
#include "log/zuclog.h"
#include "rob_common/robtool.h"
#include "rob_common/usrframe.h"
#include "errcode.h"

#include <math.h>

using axis::Axis;
using namespace axisgroup;

#define MOTFREE_INFO(fmt, ...) zuclog_info("MOTFREE", fmt, ##__VA_ARGS__)
#define MOTFREE_ERR(fmt, ...) zuclog_error("MOTFREE", fmt, ##__VA_ARGS__)
#define MOTFREE_DEBUG(fmt, ...) zuclog_debug("MOTFREE", fmt, ##__VA_ARGS__)

#ifndef MIN
#define MIN(x, y) x < y ? x : y
#endif
#ifndef MAX
#define MAX(x, y) x > y ? x : y
#endif

PFsmFree::PFsmFree(Robot* grp) : PFsmState(grp)
{
    free_tp_.resize(MAX_AXIS);
    free_tp_cart_.resize(6);
    is_in_incr = false;
}

int PFsmFree::update_plan()
{
    check_jog_cont_countdown();

    RobJointVal jcmd = grp_->cmd_rst_.jpos;
    ZucPose pcmd = grp_->cmd_rst_.pose;

    int res = 0;
    if (jog_type_ == JOG_JOINT)
    {
        res = update_joint_jog(jcmd, pcmd);
    }
    else if (jog_type_ == JOG_CART_WORLD || jog_type_ == JOG_CART_TOOL)
    {
        int count = 0;
        for (auto tp : free_tp_cart_)
        {
            if (tp.enable)
            {
                count++;
            }
        }
        if (count > 1)
        {
            MOTFREE_ERR("MultiDir is not allowed in CART JOG!");
        }
        res = update_cart_jog(jcmd, pcmd);
        if (res)
        {
            return res;
        }
    }

    grp_->cmd_rst_.jpos = jcmd;
    grp_->cmd_rst_.pose = pcmd;
    return 0;
}

int axisgroup::PFsmFree::enter()
{
    jog_cont_countdown_ = 0;
    return 0;
}

int axisgroup::PFsmFree::switch_jog_type(JogType type)
{
    if (grp_->get_state() != axisgroup::AxisGroup::FsmState::Standby)
    {
        return -1;
    }
    for (size_t i = 0; i < free_tp_.size(); i++) { free_tp_[i].enable = 0; }
    for (size_t i = 0; i < free_tp_cart_.size(); i++) { free_tp_cart_[i].enable = 0; }

    jog_type_ = type;
    if (jog_type_ == JOG_CART_WORLD || jog_type_ == JOG_CART_TOOL)
    {
        // 姿态角设置为0， 运动为增量，后续叠加到世界坐标上
        for (int i = 0; i < 6; i++) { free_tp_cart_[i].reset(0); }
        if (jog_type_ == JOG_CART_WORLD)
            for (int i = 0; i < 3; i++) { free_tp_cart_[i].reset(grp_->cmd_rst_.pose_array()[i]); }
        tool_coord_offset_ = grp_->cmd_rst_.pose;
    }
    else if (jog_type_ == JOG_JOINT)
    {
        for (size_t i = 0; i < grp_->joint_num(); i++) { free_tp_[i].reset(grp_->cmd_rst_.jpos[i]); }
    }
    else
    {
        MOTFREE_ERR("!!! UNKNOW FREE TYPE, %d", type);
        return -1;
    }
    return 0;
}

void axisgroup::PFsmFree::abort()
{
    for (size_t i = 0; i < free_tp_.size(); i++) { free_tp_[i].enable = 0; }
    for (size_t i = 0; i < free_tp_cart_.size(); i++) { free_tp_cart_[i].enable = 0; }
    is_in_incr = false;
}

bool axisgroup::PFsmFree::is_stopping()
{
    bool any_enable = false;
    for (auto tp : free_tp_)
    {
        if (tp.enable)
        {
            any_enable = true;
            break;
        }
    };
    return !any_enable && !is_plan_over();
}

int axisgroup::PFsmFree::update_joint_jog(RobJointVal& jcmd, ZucPose& pcmd)
{
    // 实际上可以通过给多个轴这是enable使得这些轴都运动
    for (size_t i = 0; i < grp_->joints.size(); i++)
    {
        auto joint = grp_->joints[i];
        auto tp = &free_tp_[i];
        //更新速度约束
        double vel_lim = MIN(joint->config().max_vel, joint->config().max_vel * grp_->config_.net_feed_scale);
        tp->max_vel = MIN(vel_lim, tp->max_vel);
        check_jog_max_vel(tp, i);  // 通过tcp速度限制计算运动关节的速度限制

        // 更新 tp->curr_pos
        simple_tp_update(tp, grp_->config_.cyclictime_sec);

        //设定目标位置
        jcmd[i] = tp->curr_pos;
    }

    grp_->kine_->fkine(
        jcmd, pcmd, &(RobotToolMgr::get_rob_tool(grp_->tool_id_).pose()), &(UsrFrameMgr::get_usr_frame(grp_->wobj_id_).world(grp_->base_frame_id_)));
    return 0;
}

int axisgroup::PFsmFree::update_cart_jog(RobJointVal& jcmd, ZucPose& pcmd)
{
    int nActiveAxis = -1;

    for (int axis_num = 0; axis_num < 6; axis_num++)
    {
        auto tp = &(free_tp_cart_[axis_num]);
        if (axis_num < 3)
        {
            tp->max_vel = MIN(tp->max_vel, grp_->get_robot_active_config().jog.vel.trans);
        }
        else
        {
            tp->max_vel = MIN(tp->max_vel, grp_->get_robot_active_config().jog.vel.ori);
        }

        check_jog_max_vel(tp, axis_num);
        simple_tp_update(tp, grp_->config_.cyclictime_sec);

        if (tp->enable == 1)
        {
            ZucPose pre_toolCoordOffset = grp_->cmd_rst_.pose;
            if (pre_check_safezone_JOG_TCP(tp, &pre_toolCoordOffset, axis_num))
            {
                // #TODO 2 类停止
                return -1;
            }
        }
        if (tp->active)
        {
            nActiveAxis = axis_num;
            break;
        }
    }
    if (nActiveAxis < 0)
    {
        // no active axis
        return 0;
    }

    // 转换为世界坐标
    if (jog_type_ == JOG_CART_WORLD)
    {
        /*note that: axis 3 means rotating around x, axis 4 means y, and axis 5 z*/
        PmPose pos_base;
        pos_base.tran.x = tool_coord_offset_.tran.x;
        pos_base.tran.y = tool_coord_offset_.tran.y;
        pos_base.tran.z = tool_coord_offset_.tran.z;
        PmRpy rpy_base;
        rpy_base.y = tool_coord_offset_.a * PM_PI / 180;
        rpy_base.p = tool_coord_offset_.b * PM_PI / 180;
        rpy_base.r = tool_coord_offset_.c * PM_PI / 180;
        pmRpyQuatConvert(&rpy_base, &pos_base.rot);

        PmPose pos_delt;
        if (nActiveAxis < 3)
        {
            pos_delt.tran.x = free_tp_cart_[0].curr_pos - tool_coord_offset_.tran.x;
            pos_delt.tran.y = free_tp_cart_[1].curr_pos - tool_coord_offset_.tran.y;
            pos_delt.tran.z = free_tp_cart_[2].curr_pos - tool_coord_offset_.tran.z;
            PmRpy rpy_delt;
            rpy_delt.y = 0;
            rpy_delt.p = 0;
            rpy_delt.r = 0;
            pmRpyQuatConvert(&rpy_delt, &pos_delt.rot);
        }
        else
        {
            PmHomogeneous hom_delt;
            PmCartesian rotDir;
            double angle = free_tp_cart_[nActiveAxis].curr_pos * PM_PI / 180;
            rotDir.x = (nActiveAxis == 3) ? 1 : 0;
            rotDir.y = (nActiveAxis == 4) ? 1 : 0;
            rotDir.z = (nActiveAxis == 5) ? 1 : 0;
            pmRotArbitAxisWithAngleConvert(&(tool_coord_offset_.tran), &rotDir, angle, &hom_delt);
            pmHomPoseConvert(&hom_delt, &pos_delt);
        }

        PmPose pos_tar;
        pmPosePoseMult(&pos_delt, &pos_base, &pos_tar);
        PmRpy rpy_tar;
        pmQuatRpyConvert(&pos_tar.rot, &rpy_tar);

        pcmd.tran.x = pos_tar.tran.x;
        pcmd.tran.y = pos_tar.tran.y;
        pcmd.tran.z = pos_tar.tran.z;
        pcmd.a = rpy_tar.y * 180 / PM_PI;
        pcmd.b = rpy_tar.p * 180 / PM_PI;
        pcmd.c = rpy_tar.r * 180 / PM_PI;
    }
    else if (jog_type_ == JOG_CART_TOOL)
    {
        PmPose pos_end;
        PmRpy rpy_end;
        pos_end.tran = tool_coord_offset_.tran;
        rpy_end.y = tool_coord_offset_.a * deg_to_rad;
        rpy_end.p = tool_coord_offset_.b * deg_to_rad;
        rpy_end.r = tool_coord_offset_.c * deg_to_rad;
        pmRpyQuatConvert(&rpy_end, &pos_end.rot);

        PmCartesian tran_delt = {0.0, 0.0, 0.0};
        PmRpy rpy_delt = {0.0, 0.0, 0.0};
        double deltS = free_tp_cart_[nActiveAxis].curr_pos;
        switch (nActiveAxis)
        {
        case 0:
            tran_delt.x = deltS;
            break;
        case 1:
            tran_delt.y = deltS;
            break;
        case 2:
            tran_delt.z = deltS;
            break;
        case 3:
            rpy_delt.y = deltS * deg_to_rad;
            break;
        case 4:
            rpy_delt.p = deltS * deg_to_rad;
            break;
        case 5:
            rpy_delt.r = deltS * deg_to_rad;
            break;
        }
        PmPose pos_delt;
        pos_delt.tran = tran_delt;
        pmRpyQuatConvert(&rpy_delt, &pos_delt.rot);
        PmPose pos_end_tar;
        pmPosePoseMult(&pos_end, &pos_delt, &pos_end_tar);
        PmRpy rpy_tar;
        pmQuatRpyConvert(&pos_end_tar.rot, &rpy_tar);

        pcmd.tran.x = pos_end_tar.tran.x;
        pcmd.tran.y = pos_end_tar.tran.y;
        pcmd.tran.z = pos_end_tar.tran.z;
        pcmd.a = rpy_tar.y * 180 / PM_PI;
        pcmd.b = rpy_tar.p * 180 / PM_PI;
        pcmd.c = rpy_tar.r * 180 / PM_PI;
    }

    kine::KineItf::kine_Conf conf = {};
    int result = grp_->kine_->ikine(
        pcmd, jcmd, conf, &(RobotToolMgr::get_rob_tool(grp_->tool_id_).pose()), &(UsrFrameMgr::get_usr_frame(grp_->wobj_id_).world(grp_->base_frame_id_)));
    if (result)
    {
        MOTFREE_ERR("Ikine failed in pose %lf, %lf, %lf, %lf, %lf, %lf", pcmd.tran.x, pcmd.tran.y, pcmd.tran.z, pcmd.a, pcmd.b, pcmd.c);
        return SINGULARITY_PROTECTION;
    }

    return 0;
}

bool axisgroup::PFsmFree::is_plan_over()
{
    bool is_planning = false;
    for (auto tp : free_tp_)
    {
        if (tp.active)
        {
            is_planning = true;
            break;
        }
    }
    for (auto tp : free_tp_cart_)
    {
        if (tp.active)
        {
            is_planning = true;
            break;
        }
    }
    return !is_planning;
}

bool axisgroup::PFsmFree::check_jog_cont_countdown()
{
    if (jog_cont_countdown_ > 0)
    {
        jog_cont_countdown_--;
        if (jog_cont_countdown_ == 0)
        {
            MOTFREE_INFO("Jog continue auto stop!");
            this->abort();
            return true;
        }
    }
    return false;
}

// 约束jog速度以满足tcp速度限制
int PFsmFree::check_jog_max_vel(simple_tp_t* tp, int jog_num)
{
    double vel_limit_tcp = 0.0;
    if (safety::SafeIOMgr::instance().is_reduce_mode())
    {
        vel_limit_tcp =
            grp_->config_.safety.max_tcp_vel < grp_->config_.safety.reduce_tcp_vel ? grp_->config_.safety.max_tcp_vel : grp_->config_.safety.reduce_tcp_vel;
    }
    else
    {
        vel_limit_tcp = grp_->config_.safety.max_tcp_vel;
    }
    if (jog_type_ != JOG_JOINT)
    {
        tp->max_vel = tp->max_vel < vel_limit_tcp ? tp->max_vel : vel_limit_tcp;
        return 0;
    }

    double r = 0.0;
    calc_jog_length(jog_num, &r);
    if (fabs(r) > 1e-6)
    {
        double max_vel_tcp = vel_limit_tcp / r / M_PI * 180.0;
        // rtapi_print(" [ZUCMOT_JOG_CONT]: net_vel = %f max_vel = %f\n", net_vel, max_vel);
        if (tp->max_vel > max_vel_tcp)
        {
            tp->max_vel = max_vel_tcp;
        }
    }
    return 0;
}

// 计算jog半径
int PFsmFree::calc_jog_length(int active_num, double* length)
{
    double delta_max = 0.0, delta_min = 0.0;
    double step = 0.0;
    delta_max = fabs(grp_->joints[active_num]->config().max_pos - grp_->cmd_rst_.jpos[active_num]);
    delta_min = fabs(grp_->joints[active_num]->config().min_pos - grp_->cmd_rst_.jpos[active_num]);

    double step_1 = 1.0;
    RobJointVal joint_pos = grp_->cmd_rst_.jpos;
    RobJointVal joint_pos_after = joint_pos;

    if (delta_max > delta_min)
    {
        step = delta_max / 2 > step_1 ? step_1 : delta_max / 2;
        joint_pos_after[active_num] = joint_pos_after[active_num] + step;
    }
    else
    {
        step = delta_min / 2 > step_1 ? step_1 : delta_min / 2;
        joint_pos_after[active_num] = joint_pos_after[active_num] - step;
    }

    ZucPose carte_pos, carte_pos_after;
    grp_->kine_->fkine(
        joint_pos, carte_pos, &(RobotToolMgr::get_rob_tool(grp_->tool_id_).pose()), &(UsrFrameMgr::get_usr_frame(grp_->wobj_id_).world(grp_->base_frame_id_)));
    grp_->kine_->fkine(joint_pos_after,
                       carte_pos_after,
                       &(RobotToolMgr::get_rob_tool(grp_->tool_id_).pose()),
                       &(UsrFrameMgr::get_usr_frame(grp_->wobj_id_).world(grp_->base_frame_id_)));
    double chord = 0;
    pmCartCartDisp(&carte_pos.tran, &carte_pos_after.tran, &chord);
    double theta_half = (step / 2) * (PM_PI / 180.0);
    if (fabs(theta_half) < 1e-9)
    {
        *length = 0.0;
        return -1;
    }
    *length = (chord / 2) / sin(theta_half);
    return 0;
}

int PFsmFree::pre_check_safezone_JOG_TCP(simple_tp_t* tp, ZucPose* toolCoordOffset, int axis_num)
{
    // // if (zucmotStatus->check_safezone_prediction == 2 || !zucmotConfig->safeZoneEnable)
    // if (!zucmotConfig->safeZoneEnable || zucmotStatus->safezone_dengrous == 1)
    // {
    //     return 0;
    // }
    // simple_tp_t tp_stop;
    // tp_stop = *tp;
    // int safeZone_elbow_wrist = 0;
    // double delta_target;
    // ZucPose joint_stop, elbow_pos_stop, pos_stop;
    // memcpy(&pos_stop, toolCoordOffset, sizeof(pos_stop));
    // double stop_pos_joint[ZUCMOT_MAX_JOINTS] = {0};
    // for (int i = 0; i < zucmotConfig->numJoints; i++) { stop_pos_joint[i] = joints[i].pos_cmd; }
    // double max_acc = (axis_num < 3) ? 3000.0 : 1080.0;
    //    // tpManager_calculate_max_acc_stop_delta_progress(tp->curr_vel, max_acc, &delta_target);
    // // char * toolCoordOffset_c = "toolCoordOffset";
    // // print_ZucPose(toolCoordOffset, 0, toolCoordOffset_c);
    // if (tp->pos_cmd > 0)
    // {
    //     tp_stop.curr_pos = tp_stop.curr_pos + delta_target;
    //     if (tp_stop.curr_pos > tp->pos_cmd)
    //     {
    //         tp_stop.curr_pos = tp->pos_cmd;
    //     }
    // }
    // else if (tp->pos_cmd < 0)
    // {
    //     tp_stop.curr_pos = tp_stop.curr_pos - delta_target;
    //     if (tp_stop.curr_pos < tp->pos_cmd)
    //     {
    //         tp_stop.curr_pos = tp->pos_cmd;
    //     }
    // }
    // PmPose pos_base;
    // pos_base.tran.x = toolCoordOffset->tran.x;
    // pos_base.tran.y = toolCoordOffset->tran.y;
    // pos_base.tran.z = toolCoordOffset->tran.z;
    // PmRpy rpy_base;
    // rpy_base.y = toolCoordOffset->a * PM_PI / 180;
    // rpy_base.p = toolCoordOffset->b * PM_PI / 180;
    // rpy_base.r = toolCoordOffset->c * PM_PI / 180;
    // pmRpyQuatConvert(&rpy_base, &pos_base.rot);
    // PmPose pos_tar;
    // ZucPose stop_pos_zucpos;
    // PmRpy rpy_tar;
    // PmRpy rpy_delt;
    // rpy_delt.y = 0;
    // rpy_delt.p = 0;
    // rpy_delt.r = 0;
    // switch (zucmotStatus->motion_state)
    // {
    // case ZUCMOT_MOTION_TELEOP: {
    //     int flag_x = (axis_num == 0) ? 1 : 0;
    //     int flag_y = (axis_num == 1) ? 1 : 0;
    //     int flag_z = (axis_num == 2) ? 1 : 0;
    //     PmPose pos_delt;
    //     if (axis_num < 3)
    //     {
    //         pos_delt.tran.x = (tp_stop.curr_pos - toolCoordOffset->tran.x) * flag_x;
    //         pos_delt.tran.y = (tp_stop.curr_pos - toolCoordOffset->tran.y) * flag_y;
    //         pos_delt.tran.z = (tp_stop.curr_pos - toolCoordOffset->tran.z) * flag_z;
    //         pmRpyQuatConvert(&rpy_delt, &pos_delt.rot);
    //     }
    //     else
    //     {
    //         PmHomogeneous hom_delt;
    //         PmCartesian rotDir;
    //         double angle = tp_stop.curr_pos * PM_PI / 180;
    //         rotDir.x = (axis_num == 3) ? 1 : 0;
    //         rotDir.y = (axis_num == 4) ? 1 : 0;
    //         rotDir.z = (axis_num == 5) ? 1 : 0;
    //         pmRotArbitAxisWithAngleConvert(&(toolCoordOffset->tran), &rotDir, angle, &hom_delt);
    //         pmHomPoseConvert(&hom_delt, &pos_delt);
    //     }
    //     // rtapi_print("pos_delt.tran.x %f\n",pos_delt.tran.x);
    //     // rtapi_print("pos_delt.tran.y %f\n",pos_delt.tran.y);
    //     // rtapi_print("pos_delt.tran.z %f\n",pos_delt.tran.z);
    //     pmPosePoseMult(&pos_delt, &pos_base, &pos_tar);
    //     pmQuatRpyConvert(&pos_tar.rot, &rpy_tar);
    //     break;
    // }
    // case ZUCMOT_MOTION_TELEOP_TOOL: {
    //     PmPose pos_end;
    //     PmRpy rpy_end;
    //     pos_end.tran = toolCoordOffset->tran;
    //     rpy_end.y = toolCoordOffset->a * PM_PI / 180.0;
    //     rpy_end.p = toolCoordOffset->b * PM_PI / 180.0;
    //     rpy_end.r = toolCoordOffset->c * PM_PI / 180.0;
    //     pmRpyQuatConvert(&rpy_end, &pos_end.rot);

    //     PmCartesian tran_delt = {0.0, 0.0, 0.0};
    //     switch (axis_num)
    //     {
    //     case 0:
    //         tran_delt.x = tp_stop.curr_pos;
    //         break;
    //     case 1:
    //         tran_delt.y = tp_stop.curr_pos;
    //         break;
    //     case 2:
    //         tran_delt.z = tp_stop.curr_pos;
    //         break;
    //     case 3:
    //         rpy_delt.y = tp_stop.curr_pos * PM_PI / 180;
    //         break;
    //     case 4:
    //         rpy_delt.p = tp_stop.curr_pos * PM_PI / 180;
    //         break;
    //     case 5:
    //         rpy_delt.r = tp_stop.curr_pos * PM_PI / 180;
    //         break;
    //     }
    //     // 1. get the target pose
    //     PmPose pos_delt;
    //     pos_delt.tran = tran_delt;
    //     pmRpyQuatConvert(&rpy_delt, &pos_delt.rot);
    //     pmPosePoseMult(&pos_end, &pos_delt, &pos_tar);
    //     pmQuatRpyConvert(&pos_tar.rot, &rpy_tar);
    //     break;
    // }
    // default: {
    //     break;
    // }
    // }
    // stop_pos_zucpos.tran.x = pos_tar.tran.x;
    // stop_pos_zucpos.tran.y = pos_tar.tran.y;
    // stop_pos_zucpos.tran.z = pos_tar.tran.z;
    // stop_pos_zucpos.a = rpy_tar.y * 180 / PM_PI;
    // stop_pos_zucpos.b = rpy_tar.p * 180 / PM_PI;
    // stop_pos_zucpos.c = rpy_tar.r * 180 / PM_PI;

    // // char * stop_pos_zucpos_c = "stop_pos_zucpos";
    // // print_ZucPose(&stop_pos_zucpos, 0, stop_pos_zucpos_c);
    // int result = kinematicsInverse(
    //     &zucmotConfig->tool_offset, &stop_pos_zucpos, stop_pos_joint, &ctrl_iflags, &ctrl_fflags, &zucmotConfig->base_offset, &zucmotConfig->user_offset);

    // if (zucmotConfig->safetyCfg.zone.elbow_pos_limit_enable)
    // {
    //     safeZone_elbow_wrist = 1;
    // }
    // kinematicsForward_elbow(stop_pos_joint, &elbow_pos_stop, &zucmotConfig->base_offset, &zucmotConfig->user_offset);

    // // kinematicsForward(
    // //             &zucmotConfig->tool_offset, stop_pos_joint, &pos_stop, &ctrl_iflags, &ctrl_fflags, &zucmotConfig->base_offset, &zucmotConfig->user_offset);
    // checkSafezone(zucmotConfig->safetyCfg.zone,
    //               stop_pos_zucpos,
    //               elbow_pos_stop,
    //               safeZone_elbow_wrist,
    //               zucmotConfig->safetyCfg.elbow_sphere_radius,
    //               zucmotConfig->user_offset,
    //               1,
    //               &zucmotStatus->checksafefalg);
    // int check_safezone = zucmotStatus->checksafefalg.pre_checkSafezone;
    // if (check_safezone)
    // {
    //     if (zucmotConfig->safetyCfg.zone.safeZoneMode == 0)
    //     {
    //         zucmotStatus->check_safezone_prediction = 1;
    //         // rtapi_print("  tp->check_safezone_prediction_s = 1 \n");
    //     }
    // }
    // // rtapi_print(" check_safezonel %d\n",check_safezone);
    // // rtapi_print(" maxaccel %f\n",max_acc);
    // // rtapi_print(" tc->progress %f\n",tp->curr_pos);
    // // rtapi_print(" tc->currentvel %f\n",tp->curr_vel);
    // // rtapi_print(" delta_target %f\n",delta_target);
    // // rtapi_print(" tp_stop.progress %f\n",tp_stop.curr_pos);
    // //rtapi_print("zucmotStatus->carte_pos_cmd.x = %f y =%f z= %f \n",zucmotStatus->carte_pos_cmd.tran.x,zucmotStatus->carte_pos_cmd.tran.y,zucmotStatus->carte_pos_cmd.tran.z);

    return 0;
}